#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "std_msgs/msg/float32.hpp"


float b=0.128, r=0.0425;
float v,w,vR,vL,rpmR,rpmL;

class RlVel : public rclcpp::Node
{
    private:
        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr r_pub_;
        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr l_pub_;
        rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_;
        void cmd_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
        {
            v=msg->linear.x;
            w=msg->angular.z;
            vR=v+b*w;
            vL=v-b*w;
            //
            rpmR=vR/r/(2*M_PI)*60;
            rpmL=vL/r/(2*M_PI)*60;
            //
            r_pub_->publish(rpmR);
            l_pub_->publish(rpmL);
        }
    public:
        RlVel(std::string name) : Node(name)
        {
            RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
            //订阅cmd_vel信息
            sub_cmd_=this->create_subscription<geometry_msgs::msg::Twist>(
                "cmd_vel",10, std::bind(&RlVel::cmd_callback,this, std::placeholders::_1));
            //发布右轮转速
            r_pub_=this->create_publisher<std_msgs::msg::Float32>("cal_vel_r",10);
            //发布左轮转速
            l_pub_=this->create_publisher<std_msgs::msg::Float32>("cal_vel_l",10);
        }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<RlVel>("rl_vel");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}